#include"vld_imu_slam/frontend.h"

namespace vld_imu_slam{

void Frontend::addFrame(shared_ptr<Frame> p_frame){
    lock_guard<mutex> lck(frame_mutex_);
    frame_deque_.push_back(p_frame);
}

bool Frontend::frontendHandle(){
    {
        lock_guard<mutex> lck(frame_mutex_);
        if(frame_deque_.empty())
            return false;
        p_current_frame_ = frame_deque_.front();
        frame_deque_.pop_front();
    }
    ROS_DEBUG_STREAM("frame id "<<p_current_frame_->frame_id_);
    switch (status_)
    {
        case FrontendStatus::INITING:
            init();
            break;
        case FrontendStatus::TRACKING_GOOD:
            ROS_DEBUG("Tracking good");
        case FrontendStatus::TRACKING_BAD:
        {
            std::chrono::system_clock::time_point t1_, t2_;
            t1_ = std::chrono::system_clock::now();
            frameRegistration();
            t2_ = std::chrono::system_clock::now();
            ROS_DEBUG_STREAM("cost time   "<<std::chrono::duration_cast<std::chrono::microseconds>( t2_-t1_ ).count()*0.001<<"ms");
            break;
        }
        case FrontendStatus::LOST:
                ROS_DEBUG("track lost");
                break;
        default:
            break;
    }
    p_last_frame_ = p_current_frame_;
    return true;
}

void Frontend::init(){
    p_current_frame_->setKeyFrame();
    p_map_->insertKeyFrame(p_current_frame_);
    p_imu_handle_->setFrameTimestamp(p_current_frame_->time_stamp_,
                                                                                    p_current_frame_->getPose()); 
    status_ = FrontendStatus::TRACKING_GOOD;
    last_kf_rpy_<<0,0,0;
    last_kf_t_<<0,0,0;
}

void Frontend::frameRegistration(){
    // 当前激光帧的角点、平面点数量足够多
    if (p_current_frame_->pc_corner_sample_->points.size() > 10 && p_current_frame_->pc_surface_sample_->points.size() > 50)
    {
        if(p_current_frame_->is_get_imu_pose_estimate_){
            cout<<"debug0"<<endl;
            double pose_array[7];
            SE3toRt(p_current_frame_->getImuPose(),pose_array);
            t_last_cur_<<pose_array[4],pose_array[5],pose_array[6];
            q_last_cur_= Eigen::Quaterniond(pose_array[3],pose_array[0],pose_array[1],pose_array[2]);
            double roll,pitch,yaw;
            tf::Quaternion q_temp(pose_array[0],pose_array[1],pose_array[2],pose_array[3]);
            tf::Matrix3x3(q_temp).getRPY(roll, pitch, yaw);
            test_rpy_<<roll,pitch,yaw;
            cout<<"debug1 "<<endl;
        }
        p_kdtree_surface_.reset(new pcl::KdTreeFLANN<PointType>);
        p_kdtree_corner_.reset(new pcl::KdTreeFLANN<PointType>);
        p_kdtree_surface_->setInputCloud(p_map_->getSurfLocalMap());
        p_kdtree_corner_->setInputCloud(p_map_->getCornerLocalMap());

        for (int iterCount = 0; iterCount < 30; iterCount++)
        {
            laserCloudOri_->clear();
            coeffSel_->clear();

            calCornerResidual();

            calSurfResidual();

            if (LMOptimization(iterCount) == true)
                break;              
        }
        p_current_frame_->setPose(SE3(q_last_cur_,t_last_cur_));
        p_imu_handle_->setFrameTimestamp(p_current_frame_->time_stamp_,
                                                                                p_current_frame_->getPose()); 
        if(judgeKeyframe()){
            p_current_frame_->setKeyFrame();
            p_map_->insertKeyFrame(p_current_frame_);
        }
    }else{
        ROS_WARN("feature num is not enough for scan to localmap registration!");
    }
}

void Frontend::calCornerResidual(){
    // cout<<"corner "<<endl;
    Eigen::Matrix4d T1 = Eigen::Matrix4d::Identity();
    T1.block<3, 3>(0, 0) = q_last_cur_.toRotationMatrix();
    T1.block<3, 1>(0, 3) = t_last_cur_;

    #pragma omp parallel for num_threads(p_config_->numberOfCores_)
    for(int pt_idx =0; pt_idx<p_current_frame_->pc_corner_sample_->points.size();pt_idx++){
        PointType pt_tranform;
        PointType coeff;

        vector<int> point_search_Idx;
        vector<float> point_search_dis;
        
        tranformPoint(p_current_frame_->pc_corner_sample_->points[pt_idx], pt_tranform, T1);
        // 从上一帧中寻找最近邻点
        p_kdtree_corner_->nearestKSearch(pt_tranform, 5, point_search_Idx, point_search_dis);
        // 确保搜索点足够近
        if(point_search_dis[4] < max_dis_plane_for_match_)
        {
            // 计算5个点的均值坐标，记为中心点
            float cx = 0, cy = 0, cz = 0;
            for (int j = 0; j < 5; j++) {
                cx += p_map_->getCornerLocalMap()->points[point_search_Idx[j]].x;
                cy += p_map_->getCornerLocalMap()->points[point_search_Idx[j]].y;
                cz += p_map_->getCornerLocalMap()->points[point_search_Idx[j]].z;
            }
            cx /= 5; cy /= 5;  cz /= 5;

            // 计算协方差
            float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;
            for (int j = 0; j < 5; j++) {
                // 计算点与中心点之间的距离
                float ax = p_map_->getCornerLocalMap()->points[point_search_Idx[j]].x - cx;
                float ay = p_map_->getCornerLocalMap()->points[point_search_Idx[j]].y - cy;
                float az = p_map_->getCornerLocalMap()->points[point_search_Idx[j]].z - cz;

                a11 += ax * ax; a12 += ax * ay; a13 += ax * az;
                a22 += ay * ay; a23 += ay * az;
                a33 += az * az;
            }
            a11 /= 5; a12 /= 5; a13 /= 5; a22 /= 5; a23 /= 5; a33 /= 5;

            Eigen::Matrix3d covMat;
            covMat(0,0) = a11; covMat(0,1) = a12; covMat(0,2) = a13;
            covMat(1,0) = a12; covMat(1,1) = a22; covMat(1,2) = a23;
            covMat(2,0) = a13; covMat(2,1) = a23; covMat(2,2) = a33;

            // SVD分解协方差，如果最大特征值远大于其余两个特征值，说明分布成直线
            // 最大特征值对应特征向量即为直线的方向
            Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(covMat);
            // note Eigen library sort eigenvalues in increasing order
            Eigen::Vector3d unit_direction = saes.eigenvectors().col(2);
            if(saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1])   //这个必须判断 不然容易出现奇异值
            {
                // 根据直线方向和中心点，在直线上找两个点
                Eigen::Vector3d point_on_line(cx,cy,cz);
                // 提取到的直线上的两个点，用于确定方向
                Eigen::Vector3d tmp_a, tmp_b;
                tmp_a = 0.1 * Eigen::Vector3d( unit_direction[0], unit_direction[1], unit_direction[2]) + point_on_line;
                tmp_b = -0.1 * Eigen::Vector3d( unit_direction[0], unit_direction[1], unit_direction[2]) + point_on_line; 

                Eigen::Vector3d tmp_p(pt_tranform.x, pt_tranform.y, pt_tranform.z);
                Eigen::Matrix<double, 3, 1> nu = (tmp_p - tmp_a).cross(tmp_p - tmp_b); //(叉乘)
                Eigen::Matrix<double, 3, 1> de = tmp_a - tmp_b;
                Eigen::Matrix<double, 3, 1> ld = de.cross(nu);
                ld.normalize();
                coeff.x =  ld[0]; 
                coeff.y = ld[1];
                coeff.z =  ld[2];
                coeff.intensity = nu.norm()/de.norm();
                // cout<<"coeff "<<coeff<<endl;
                // cout<<"point raw "<<p_current_frame_->pc_corner_sample_->points[pt_idx]<<endl;
                // 距离越大，s越小，是个距离惩罚因子（权重）
                float s = 1 - 0.9 * fabs(coeff.intensity);

                // 点到直线的垂线段单位向量
                coeff.x = s * coeff.x;
                coeff.y = s * coeff.y;
                coeff.z = s * coeff.z;
                // 点到直线距离
                coeff.intensity = s * coeff.intensity;

                if (s > 0.1) {
                    // cout<<"coeff "<<coeff<<endl;
                    // cout<<"point raw "<<p_current_frame_->pc_corner_sample_->points[pt_idx]<<endl;
                    lock_guard<mutex> lck(cal_resd_mutex_);
                    coeffSel_->push_back(coeff);
                    laserCloudOri_->push_back(p_current_frame_->pc_corner_sample_->points[pt_idx]);
                }
            }
        }
    }
}

void Frontend::calSurfResidual(){
    // cout<<"surf "<<endl;
    Eigen::Matrix4d T1 = Eigen::Matrix4d::Identity();
    T1.block<3, 3>(0, 0) = q_last_cur_.toRotationMatrix();
    T1.block<3, 1>(0, 3) = t_last_cur_;

    #pragma omp parallel for num_threads(p_config_->numberOfCores_)
    for(int pt_idx =0; pt_idx<p_current_frame_->pc_surface_sample_->points.size();pt_idx++){
        PointType pt_tranform;
        PointType coeff;

        vector<int> point_search_Idx;
        vector<float> point_search_dis;

        tranformPoint(p_current_frame_->pc_surface_sample_->points[pt_idx], pt_tranform, T1);
        // 从上一帧中寻找最近邻点
        p_kdtree_surface_->nearestKSearch(pt_tranform, 5, point_search_Idx, point_search_dis);
        Eigen::Matrix<float, 5, 3> matA0;
        Eigen::Matrix<float, 5, 1> matB0;
        Eigen::Vector3f matX0;

        matA0.setZero();
        matB0.fill(-1);
        matX0.setZero();
        // 确保搜索点足够近
        if(point_search_dis[4] < max_dis_plane_for_match_)
        {
            for (int j = 0; j < 5; j++) {
                matA0(j, 0) = p_map_->getSurfLocalMap()->points[point_search_Idx[j]].x;
                matA0(j, 1) = p_map_->getSurfLocalMap()->points[point_search_Idx[j]].y;
                matA0(j, 2) = p_map_->getSurfLocalMap()->points[point_search_Idx[j]].z;
            }

            // 假设平面方程为ax+by+cz+1=0，这里就是求方程的系数abc，d=1
            matX0 = matA0.colPivHouseholderQr().solve(matB0);

            // 平面方程的系数，也是法向量的分量
            float pa = matX0(0, 0);
            float pb = matX0(1, 0);
            float pc = matX0(2, 0);
            float pd = 1;

            // 单位法向量
            float ps = sqrt(pa * pa + pb * pb + pc * pc);
            pa /= ps; pb /= ps; pc /= ps; pd /= ps;

            // 检查平面是否合格，如果5个点中有点到平面的距离超过0.2m，那么认为这些点太分散了，不构成平面
            bool planeValid = true;
            for (int j = 0; j < 5; j++) {
                if (fabs(pa * p_map_->getSurfLocalMap()->points[point_search_Idx[j]].x +
                            pb * p_map_->getSurfLocalMap()->points[point_search_Idx[j]].y +
                            pc * p_map_->getSurfLocalMap()->points[point_search_Idx[j]].z + pd) > 0.2) {
                    planeValid = false;
                    break;
                }
            }

            if(planeValid)
            {
                Eigen::Vector3d target_line_a( p_map_->getSurfLocalMap()->points[point_search_Idx[0]].x,
                                                        p_map_->getSurfLocalMap()->points[point_search_Idx[0]].y,
                                                        p_map_->getSurfLocalMap()->points[point_search_Idx[0]].z);
                Eigen::Vector3d target_line_b( p_map_->getSurfLocalMap()->points[point_search_Idx[2]].x,
                                                        p_map_->getSurfLocalMap()->points[point_search_Idx[2]].y,
                                                        p_map_->getSurfLocalMap()->points[point_search_Idx[2]].z);
                Eigen::Vector3d target_line_c( p_map_->getSurfLocalMap()->points[point_search_Idx[4]].x,
                                                        p_map_->getSurfLocalMap()->points[point_search_Idx[4]].y,
                                                        p_map_->getSurfLocalMap()->points[point_search_Idx[4]].z);
                Eigen::Vector3d m_unit_vec_ab,m_unit_vec_ac,m_unit_vec_normal,vec_ad;
                m_unit_vec_ab = target_line_b - target_line_a;
                m_unit_vec_ab = m_unit_vec_ab / m_unit_vec_ab.norm();

                m_unit_vec_ac = target_line_c - target_line_a;
                m_unit_vec_ac = m_unit_vec_ac / m_unit_vec_ac.norm();

                m_unit_vec_normal = m_unit_vec_ab.cross(m_unit_vec_ac);
                m_unit_vec_normal.normalize();

                Eigen::Vector3d pt_transfromed(pt_tranform.x, pt_tranform.y, pt_tranform.z);
                vec_ad = pt_transfromed - target_line_a;

                coeff.x = m_unit_vec_normal[0];
                coeff.y = m_unit_vec_normal[1];
                coeff.z = m_unit_vec_normal[2];
                coeff.intensity = vec_ad.transpose()* m_unit_vec_normal;
                // 距离越大，s越小，是个距离惩罚因子（权重）
                float s = 1 - 0.9 * fabs(coeff.intensity);

                // 点到直线的垂线段单位向量
                coeff.x = s * coeff.x;
                coeff.y = s * coeff.y;
                coeff.z = s * coeff.z;
                // 点到直线距离
                coeff.intensity = s * coeff.intensity;

                // cout<<"coeff "<<coeff<<endl;
                // cout<<"point raw "<<p_current_frame_->pc_surface_sample_->points[pt_idx]<<endl;
                if (s > 0.1) {        
                    lock_guard<mutex> lck(cal_resd_mutex_);
                    coeffSel_->push_back(coeff);
                    laserCloudOri_->push_back(p_current_frame_->pc_surface_sample_->points[pt_idx]);
                }
            }
        }
    }
}

bool Frontend::LMOptimization(int iterCount)
{
    // This optimization is from the original loam_velodyne by Ji Zhang, need to cope with coordinate transformation
    // lidar <- camera      ---     camera <- lidar
    // x = z                ---     x = y
    // y = x                ---     y = z
    // z = y                ---     z = x
    // roll = yaw           ---     roll = pitch
    // pitch = roll         ---     pitch = yaw
    // yaw = pitch          ---     yaw = roll

    // lidar -> camera
    // cout<<"raw test rpy "<<test_rpy_<<"raw t last cur "<<t_last_cur_<<endl;
    float srx = sin(test_rpy_[1]);
    float crx = cos(test_rpy_[1]);
    float sry = sin(test_rpy_[2]);
    float cry = cos(test_rpy_[2]);
    float srz = sin(test_rpy_[0]);
    float crz = cos(test_rpy_[0]);

    // 当前帧匹配特征点数太少
    int laserCloudSelNum = laserCloudOri_->size();
    if (laserCloudSelNum < 50) {
        return false;
    }

    cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0));
    cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0));
    cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));
    cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0));
    cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));
    cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));

    PointType pointOri, coeff;

    // 遍历匹配特征点，构建Jacobian矩阵
    for (int i = 0; i < laserCloudSelNum; i++) {
        // lidar -> camera todo
        pointOri.x = laserCloudOri_->points[i].y;
        pointOri.y = laserCloudOri_->points[i].z;
        pointOri.z = laserCloudOri_->points[i].x;
        // lidar -> camera
        coeff.x = coeffSel_->points[i].y;
        coeff.y = coeffSel_->points[i].z;
        coeff.z = coeffSel_->points[i].x;
        coeff.intensity = coeffSel_->points[i].intensity;
        // in camera
        float arx = (crx*sry*srz*pointOri.x + crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x
                    + (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y
                    + (crx*cry*srz*pointOri.x + crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z;

        float ary = ((cry*srx*srz - crz*sry)*pointOri.x 
                    + (sry*srz + cry*crz*srx)*pointOri.y + crx*cry*pointOri.z) * coeff.x
                    + ((-cry*crz - srx*sry*srz)*pointOri.x 
                    + (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z;

        float arz = ((crz*srx*sry - cry*srz)*pointOri.x + (-cry*crz-srx*sry*srz)*pointOri.y)*coeff.x
                    + (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y
                    + ((sry*srz + cry*crz*srx)*pointOri.x + (crz*sry-cry*srx*srz)*pointOri.y)*coeff.z;
        // lidar -> camera
        matA.at<float>(i, 0) = arz;
        matA.at<float>(i, 1) = arx;
        matA.at<float>(i, 2) = ary;
        matA.at<float>(i, 3) = coeff.z;
        matA.at<float>(i, 4) = coeff.x;
        matA.at<float>(i, 5) = coeff.y;
        // 点到直线距离、平面距离，作为观测值
        matB.at<float>(i, 0) = -coeff.intensity;
    }

    cv::transpose(matA, matAt);
    matAtA = matAt * matA;
    matAtB = matAt * matB;
    // J^T·J·delta_x = -J^T·f 高斯牛顿
    cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);
    // cout<<"raw delta "<<matX<<endl;

    // 首次迭代，检查近似Hessian矩阵（J^T·J）是否退化，或者称为奇异，行列式值=0 todo
    if (iterCount == 0) {

        cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0));
        cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0));
        cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0));

        cv::eigen(matAtA, matE, matV);
        matV.copyTo(matV2);

        is_degenerate_ = false;
        float eignThre[6] = {10, 10, 10, 10, 10, 10};
        for (int i = 5; i >= 0; i--) {
            if (matE.at<float>(0, i) < eignThre[i]) {
                for (int j = 0; j < 6; j++) {
                    matV2.at<float>(i, j) = 0;
                }
                is_degenerate_ = true;
            } else {
                break;
            }
        }
        // cout<<"matE "<<matE<<endl;
        // cout<<"matV "<<matV2 <<endl;
        // cout<<"matV inv"<<matV.inv()  <<endl;
        matP_ = matV.inv() * matV2;
        // cout<<"P "<<matP_<<endl;
    }

    if (is_degenerate_)
    {
        cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));
        matX.copyTo(matX2);
        matX = matP_ * matX2;
        // cout<<"pp "<<matP_<<endl;
    }

    // cout<<"delta "<<matX<<endl;

    // 更新当前位姿 x = x + delta_x
    // test_rpy_[0] += 0;
    test_rpy_[0] += matX.at<float>(0, 0);
    test_rpy_[1] += matX.at<float>(1, 0);
    test_rpy_[2] += matX.at<float>(2, 0);
    t_last_cur_[0] += matX.at<float>(3, 0);
    t_last_cur_[1] += matX.at<float>(4, 0);
    t_last_cur_[2] +=  matX.at<float>(5, 0);
    // t_last_cur_[2] += 0;
    Eigen::AngleAxisd axis_xAngle(matX.at<float>(0, 0), Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd axis_yAngle(matX.at<float>(1, 0), Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd axis_zAngle(matX.at<float>(2, 0), Eigen::Vector3d::UnitZ());
    q_last_cur_ = q_last_cur_*axis_xAngle*axis_yAngle*axis_zAngle;
    // q_last_cur_ = q_last_cur_*axis_zAngle;
    // cout<<"test rpy "<<test_rpy_<<" t last cur "<<t_last_cur_<<endl;

    float deltaR = sqrt(
                        pow(pcl::rad2deg(matX.at<float>(0, 0)), 2) +
                        pow(pcl::rad2deg(matX.at<float>(1, 0)), 2) +
                        pow(pcl::rad2deg(matX.at<float>(2, 0)), 2));
    float deltaT = sqrt(
                        pow(matX.at<float>(3, 0) * 100, 2) +
                        pow(matX.at<float>(4, 0) * 100, 2) +
                        pow(matX.at<float>(5, 0) * 100, 2));

    // delta_x很小，认为收敛
    if (deltaR < 0.05 && deltaT < 0.05) {
        return true; 
    }
    return false; 
}

bool  Frontend::judgeKeyframe(){
    if(abs(test_rpy_[0] - last_kf_rpy_[0])<p_config_->kf_angle_threshold_&&
    abs(test_rpy_[1] - last_kf_rpy_[1])<p_config_->kf_angle_threshold_&&
    abs(test_rpy_[2] - last_kf_rpy_[2])<p_config_->kf_angle_threshold_&&
    (t_last_cur_ - last_kf_t_).norm()<p_config_->kf_trans_threshold_){
        return false;
    }else{
        last_kf_rpy_ = test_rpy_;
        last_kf_t_ = t_last_cur_;
        return true;
    }
}

// bool Frontend::LMOptimization(int iterCount)
// {
//     // This optimization is from the original loam_velodyne by Ji Zhang, need to cope with coordinate transformation
//     // lidar <- camera      ---     camera <- lidar
//     // x = z                ---     x = y
//     // y = x                ---     y = z
//     // z = y                ---     z = x
//     // roll = yaw           ---     roll = pitch
//     // pitch = roll         ---     pitch = yaw
//     // yaw = pitch          ---     yaw = roll

//     // lidar -> camera
//     // cout<<"raw test rpy "<<test_rpy_<<"raw t last cur "<<t_last_cur_<<endl;
//     float srx = sin(test_rpy_[1]);
//     float crx = cos(test_rpy_[1]);
//     float sry = sin(test_rpy_[2]);
//     float cry = cos(test_rpy_[2]);
//     float srz = sin(test_rpy_[0]);
//     float crz = cos(test_rpy_[0]);

//     // 当前帧匹配特征点数太少
//     int laserCloudSelNum = laserCloudOri_->size();
//     if (laserCloudSelNum <= 0) {
//         return false;
//     }

//     Eigen::Matrix<double,6,6> H;
//     Eigen::Matrix<double,6,1> J_t_B,delta_x;
//     Eigen::MatrixXd J,B;
//     J=Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>();
//     J.resize(laserCloudSelNum,6);
//     B=Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>();
//     B.resize(laserCloudSelNum,1);

//     PointType pointOri, coeff;

//     // 遍历匹配特征点，构建Jacobian矩阵
//     for (int i = 0; i < laserCloudSelNum; i++) {
//         // lidar -> camera todo
//         pointOri.x = laserCloudOri_->points[i].y;
//         pointOri.y = laserCloudOri_->points[i].z;
//         pointOri.z = laserCloudOri_->points[i].x;
//         // lidar -> camera
//         coeff.x = coeffSel_->points[i].y;
//         coeff.y = coeffSel_->points[i].z;
//         coeff.z = coeffSel_->points[i].x;
//         coeff.intensity = coeffSel_->points[i].intensity;
//         // in camera
//         float arx = (crx*sry*srz*pointOri.x + crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x
//                     + (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y
//                     + (crx*cry*srz*pointOri.x + crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z;

//         float ary = ((cry*srx*srz - crz*sry)*pointOri.x 
//                     + (sry*srz + cry*crz*srx)*pointOri.y + crx*cry*pointOri.z) * coeff.x
//                     + ((-cry*crz - srx*sry*srz)*pointOri.x 
//                     + (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z;

//         float arz = ((crz*srx*sry - cry*srz)*pointOri.x + (-cry*crz-srx*sry*srz)*pointOri.y)*coeff.x
//                     + (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y
//                     + ((sry*srz + cry*crz*srx)*pointOri.x + (crz*sry-cry*srx*srz)*pointOri.y)*coeff.z;
//         // lidar -> camera
//         J(i, 0) = arz;
//         J(i, 1) = arx;
//         J(i, 2) = ary;
//         J(i, 3) = coeff.z;
//         J(i, 4) = coeff.x;
//         J(i, 5) = coeff.y;
//         // 点到直线距离、平面距离，作为观测值
//         B(i, 0) = -coeff.intensity;
//     }

//     H = J.transpose()*J;
//     J_t_B = J.transpose()*B;
//     delta_x = H.colPivHouseholderQr().solve(J_t_B);
//     cout<<"H "<<H<<endl;
//     cout<<"J_t_B "<<J_t_B<<endl;
//     cout<<"delta x "<<delta_x<<endl;

//     // 首次迭代，检查近似Hessian矩阵（J^T·J）是否退化，或者称为奇异，行列式值=0 todo
//     if(iterCount == 0)
//     {
//         Eigen::Matrix<double, 1, 6> matE = Eigen::Matrix<double, 1, 6>::Zero();
//         Eigen::Matrix<double, 6, 6> matV = Eigen::Matrix<double, 6, 6>::Zero();
//         Eigen::Matrix<double, 6, 6> matV2 = Eigen::Matrix<double, 6, 6>::Zero();

//         // 计算At*A的特征值和特征向量
//         // 特征值存放在matE，特征向量matV
//         Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 6, 6>> eigenSolver(H);
//         matE = eigenSolver.eigenvalues();
//         matV = eigenSolver.eigenvectors();

//         matV2 = matV;
//         // 退化的具体表现是指什么？
//         is_degenerate_ = false;
//         double eignThre[6] = {100, 100, 100};
//         for (int i = 5; i >= 0; i--) {
//             if (matE(0, i) < eignThre[i]) {
//                 for (int j = 0; j < 6; j++) {
//                     matV2(i, j) = 0;
//                 }
//                 // 存在比10小的特征值则出现退化
//                 is_degenerate_ = true;
//             } else {
//                 break;
//             }
//         }
//         matP_ = matV.inverse() * matV2;
//     }

//     if (is_degenerate_) {
//         Eigen::Matrix<double, 6, 1> matX2 = Eigen::Matrix<double, 6, 1>::Zero();
//         matX2 = delta_x;
//         delta_x = matP_ * matX2;
//     }

//     // 更新当前位姿 x = x + delta_x
//     test_rpy_[0] += 0;
//     // test_rpy_[0] += delta_x(0, 0);
//     test_rpy_[1] += delta_x(1, 0);
//     test_rpy_[2] += delta_x(2, 0);
//     t_last_cur_[0] += delta_x(3, 0);
//     t_last_cur_[1] += delta_x(4, 0);
//     // t_last_cur_[2] += delta_x(5, 0);
//     t_last_cur_[2] += 0;
//     Eigen::AngleAxisd axis_xAngle(delta_x[0], Eigen::Vector3d::UnitX());
//     Eigen::AngleAxisd axis_yAngle(delta_x[1], Eigen::Vector3d::UnitY());
//     Eigen::AngleAxisd axis_zAngle(delta_x[2], Eigen::Vector3d::UnitZ());
//     q_last_cur_ = q_last_cur_*axis_xAngle*axis_yAngle*axis_zAngle;
//     cout<<"test rpy "<<test_rpy_<<" t last cur "<<t_last_cur_<<endl;

//     float deltaR = sqrt(
//                         pow(pcl::rad2deg(delta_x(0, 0)), 2) +
//                         pow(pcl::rad2deg(delta_x(1, 0)), 2) +
//                         pow(pcl::rad2deg(delta_x(2, 0)), 2));
//     float deltaT = sqrt(
//                         pow(delta_x(3, 0) * 100, 2) +
//                         pow(delta_x(4, 0) * 100, 2) +
//                         pow(delta_x(5, 0) * 100, 2));

//     // delta_x很小，认为收敛
//     if (deltaR < 0.05 && deltaT < 0.05) {
//         return true; 
//     }
//     return false; 
// }

// void Frontend::frameRegistration(){
//     if (p_current_frame_->pc_cornerPt_sample_->size() < 10 || p_current_frame_->pc_groundPt_sample_->size() < 100)
//         return;
    
//     p_kdtree_ground_.reset(new pcl::KdTreeFLANN<PointType>);
//     p_kdtree_ground_->setInputCloud(p_last_frame_->pc_ground_);
//     p_kdtree_corner_.reset(new pcl::KdTreeFLANN<PointType>);
//     p_kdtree_corner_->setInputCloud(p_last_frame_->pc_corner_);

//     for (int iterCount1 = 0; iterCount1 < 25; iterCount1++) {
//         // 找到对应的特征平面
//         // 然后计算协方差矩阵，保存在coeffSel队列中
//         // laserCloudOri中保存的是对应于coeffSel的未转换到开始时刻的原始点云数据
//         calSurfResidual(iterCount1);
//         if (laserCloudOri_->points.size() < 10)
//             continue;
//         // 通过面特征的匹配，计算变换矩阵
//         if (calSurfJacobi(iterCount1) == false)
//             break;
//     }

//     for (int iterCount2 = 0; iterCount2 < 25; iterCount2++) {
//         // 找到对应的特征边/角点
//         // 寻找边特征的方法和寻找平面特征的很类似，过程可以参照寻找平面特征的注释
//         calCornerResidual(iterCount2);
//         if (laserCloudOri_->points.size() < 10)
//             continue;
//         // 通过角/边特征的匹配，计算变换矩阵
//         if (calCornerJacobi(iterCount2) == false)
//             break;
//     }
//     p_current_frame_->setPose(SE3(q_last_cur_,t_last_cur_));
//     ROS_DEBUG_STREAM("frame pose "<<SE3toEigenM(p_current_frame_->getPose()));
//     p_current_frame_->setKeyFrame();
//     p_map_->insertKeyFrame(p_current_frame_);
// }

// void Frontend::calSurfResidual(int iterCount){
//     Eigen::Matrix4d T1 = Eigen::Matrix4d::Identity();
//     T1.block<3, 3>(0, 0) = q_last_cur_.toRotationMatrix();
//     T1.block<3, 1>(0, 3) = t_last_cur_;

//     coeffSel_->clear();
//     if(iterCount % 5 != 0&&!laserCloudOri_->empty()){
//         pcl::transformPointCloud(*laserCloudOri_, *laserCloudOri_, T1);
//     } else{
//         vector<int> point_search_Idx;
//         vector<float> point_search_dis;
//         pcl::PointCloud<PointType>::Ptr pc_temp(new pcl::PointCloud<PointType>);

//         laserCloudOri_->clear();
//         plane_p_normal_set_.clear();
//         pcl::transformPointCloud(*p_current_frame_->pc_groundPt_sample_, *pc_temp, T1);
//         for (auto & pt : pc_temp->points) {
//             p_kdtree_ground_->nearestKSearch(pt, 5, point_search_Idx, point_search_dis);
//             if(point_search_Idx[4] < max_dis_plane_for_match_)
//             {
//                 std::vector<Eigen::Vector3f> nearCorners;
//                 Eigen::Vector3f center(0.0, 0.0, 0.0);
//                 for(int j=0; j<5; j++)
//                 {
//                     Eigen::Vector3f tmp(
//                         p_last_frame_->pc_ground_->points[point_search_Idx[j]].x,
//                         p_last_frame_->pc_ground_->points[point_search_Idx[j]].y,
//                         p_last_frame_->pc_ground_->points[point_search_Idx[j]].z
//                     );
//                     center = center + tmp;
//                     nearCorners.push_back(tmp);
//                 }
//                 center = center / 5.0;

//                 Eigen::Matrix3f covMat = Eigen::Matrix3f::Zero();
//                 for(int j=0; j<5; j++)
//                 {
//                     Eigen::Vector3f tmpZeroMean = 
//                         nearCorners[j] - center;
//                     covMat = covMat + tmpZeroMean * tmpZeroMean.transpose();
//                 }

//                 // SVD分解协方差，如果最大特征值平面判断
//                 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> saes(covMat);
//                 if((saes.eigenvalues()[2] > 3 * saes.eigenvalues()[0]) &&
//                     (saes.eigenvalues()[2] < 10 * saes.eigenvalues()[1]))
//                 {
//                     Eigen::Vector3d target_line_a( p_last_frame_->pc_ground_->points[point_search_Idx[0]].x,
//                                                             p_last_frame_->pc_ground_->points[point_search_Idx[0]].y,
//                                                             p_last_frame_->pc_ground_->points[point_search_Idx[0]].z);
//                     Eigen::Vector3d target_line_b( p_last_frame_->pc_ground_->points[point_search_Idx[2]].x,
//                                                             p_last_frame_->pc_ground_->points[point_search_Idx[2]].y,
//                                                             p_last_frame_->pc_ground_->points[point_search_Idx[2]].z);
//                     Eigen::Vector3d target_line_c( p_last_frame_->pc_ground_->points[point_search_Idx[4]].x,
//                                                             p_last_frame_->pc_ground_->points[point_search_Idx[4]].y,
//                                                             p_last_frame_->pc_ground_->points[point_search_Idx[4]].z);
//                     Eigen::Vector3d m_unit_vec_ab,m_unit_vec_ac,m_unit_vec_normal,vec_ad;
//                     m_unit_vec_ab = target_line_b - target_line_a;
//                     m_unit_vec_ab = m_unit_vec_ab / m_unit_vec_ab.norm();

//                     m_unit_vec_ac = target_line_c - target_line_a;
//                     m_unit_vec_ac = m_unit_vec_ac / m_unit_vec_ac.norm();

//                     m_unit_vec_normal = m_unit_vec_ab.cross(m_unit_vec_ac);
//                     m_unit_vec_normal.normalize();
//                     plane_p_normal_set_.emplace_back(make_pair(target_line_a,m_unit_vec_normal));
//                     // 当前帧的特征点
//                     laserCloudOri_->push_back(pt);
//                 }
//             }
//         }
//     }

//     PointType coeff;
//     pcl::PointCloud<PointType>::Ptr pt_select(new pcl::PointCloud<PointType>);
//     vector<pair<Eigen::Vector3d,Eigen::Vector3d>>  plane_p_normal_selsct;
//     for(int i=0;i<laserCloudOri_->points.size();i++){
//         Eigen::Vector3d pt_transfromed(laserCloudOri_->points[i].x, laserCloudOri_->points[i].y, laserCloudOri_->points[i].z);
//         Eigen::Vector3d vec_ad;
//         vec_ad = pt_transfromed - plane_p_normal_set_[i].first;
//         coeff.x =plane_p_normal_set_[i].second[0];
//         coeff.y =plane_p_normal_set_[i].second[1];
//         coeff.z =plane_p_normal_set_[i].second[2];
//         coeff.intensity = vec_ad.transpose()*plane_p_normal_set_[i].second;
//         //必须保证残差为正值，同时，法向量指向平面
//         if(coeff.intensity>0){
//             // coeff.x = -coeff.x;
//             // coeff.y = -coeff.y;
//             // coeff.z = -coeff.z;
//         }else{
//             coeff.intensity = -coeff.intensity;
//         }
//         //权重计算，距离越大权重越小，距离越小权重越大，得到的权重范围<=1
//         float s = 1;
//         if (iterCount >= 0) { //5次迭代之后开始增加权重因素
//             //增加权重，距离越远，影响影子越小
//             s = 1 - 1.8 * fabs(coeff.intensity);
//         }
//         // if(fabs(ld2) > 1.8)
//         //     s = 0.0;

//         if (s > 0.1 && coeff.intensity != 0) {
//             coeff.x = s * coeff.x;
//             coeff.y = s * coeff.y;
//             coeff.z = s * coeff.z;
//             coeff.intensity = s * coeff.intensity;
//             // 特征点指到特征直线的垂线方向，其中intensity存储了点到直线的距离，也就是残差
//             coeffSel_->push_back(coeff);
//             pt_select->push_back(laserCloudOri_->points[i]);
//             plane_p_normal_selsct.emplace_back(plane_p_normal_set_[i]);
//         }
//     }
//     pcl::copyPointCloud(*pt_select,*laserCloudOri_);
//     plane_p_normal_set_ = plane_p_normal_selsct;
// }

// bool Frontend::calSurfJacobi(int iterCount){
//     int pointSelNum = laserCloudOri_->points.size();
//     Eigen::Matrix<double, 1, 3> coeff;
//     Eigen::Matrix<double,3,3> m_temp,H;
//     Eigen::Vector3d pointOri,J_t_B,delta_x;
//     Eigen::Matrix<double, 3, 3> matP = Eigen::Matrix<double, 3, 3>::Zero();
//     Eigen::MatrixXd J,B;
//     J=Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>();
//     J.resize(pointSelNum,3);
//     B=Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>();
//     B.resize(pointSelNum,1);
//     // 构建雅可比矩阵，求解
//     for (int i = 0; i < pointSelNum; i++) {
//         pointOri[0] = laserCloudOri_->points[i].x;
//         pointOri[1] = laserCloudOri_->points[i].y;
//         pointOri[2] = laserCloudOri_->points[i].z;
//         coeff[0] = coeffSel_->points[i].x;
//         coeff[1] = coeffSel_->points[i].y;
//         coeff[2] = coeffSel_->points[i].z;

//         anti_symmetric(q_last_cur_.toRotationMatrix()*pointOri, m_temp);
//         m_temp = -m_temp;
//         coeff = coeff*m_temp;
        
//         J(i,0) = coeff[0];
//         J(i,1) = coeff[1];
//         J(i,2) = coeffSel_->points[i].z;
//         B(i, 0) =- 0.05 * coeffSel_->points[i].intensity;   //原代码乘了-0.05
//     }
//     H = J.transpose()*J;
//     J_t_B = J.transpose()*B;
//     delta_x = H.colPivHouseholderQr().solve(J_t_B);

//     if(iterCount == 0)
//     {
//         Eigen::Matrix<double, 1, 3> matE = Eigen::Matrix<double, 1, 3>::Zero();
//         Eigen::Matrix<double, 3, 3> matV = Eigen::Matrix<double, 3, 3>::Zero();
//         Eigen::Matrix<double, 3, 3> matV2 = Eigen::Matrix<double, 3, 3>::Zero();

//         // 计算At*A的特征值和特征向量
//         // 特征值存放在matE，特征向量matV
//         Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigenSolver(H);
//         matE = eigenSolver.eigenvalues();
//         matV = eigenSolver.eigenvectors();

//         matV2 = matV;
//         // 退化的具体表现是指什么？
//         is_degenerate_ = false;
//         double eignThre[3] = {10, 10, 10};
//         for (int i = 2; i >= 0; i--) {
//             if (matE(0, i) < eignThre[i]) {
//                 for (int j = 0; j < 3; j++) {
//                     matV2(i, j) = 0;
//                 }
//                 // 存在比10小的特征值则出现退化
//                 is_degenerate_ = true;
//             } else {
//                 break;
//             }
//         }
//         matP = matV.inverse() * matV2;
//     }

//     if (is_degenerate_) {
//         Eigen::Matrix<double, 3, 1> matX2 = Eigen::Matrix<double, 3, 1>::Zero();
//         // matX.copyTo(matX2);
//         matX2 = delta_x;
//         delta_x = matP * matX2;
//     }

//     for(int i=0; i<3; i++){
//         if(isnan(delta_x[i]||delta_x[i]>0.2))
//             delta_x[i]=0;
//     }

//     cout<<"ground delta x "<<delta_x<<endl;
//     test_rpy_[0] += delta_x[0];
//     test_rpy_[1] += delta_x[1];
//     Eigen::AngleAxisd axis_xAngle(delta_x[0], Eigen::Vector3d::UnitX());
//     Eigen::AngleAxisd axis_yAngle(delta_x[1], Eigen::Vector3d::UnitY());
//     q_last_cur_ = (q_last_cur_ * axis_xAngle*axis_yAngle).normalized();
//     t_last_cur_[2] += delta_x[2];

//     float deltaR = sqrt(
//                         pow(delta_x[0]*180/M_PI, 2) +
//                         pow(delta_x[1]*180/M_PI, 2));
//     float deltaT = sqrt(
//                         pow(delta_x[3] * 100, 2));

//     if (deltaR < 0.1 && deltaT < 0.1) {
//         return false;
//     }
//     return true;
// }


// void Frontend::calCornerResidual(int iterCount){
//     laserCloudOri_->clear();
//     coeffSel_->clear();

//     Eigen::Matrix4d T1 = Eigen::Matrix4d::Identity();
//     T1.block<3, 3>(0, 0) = q_last_cur_.toRotationMatrix();
//     T1.block<3, 1>(0, 3) = t_last_cur_;
//     PointType pt_tranform;

//     if(iterCount % 5 == 0){
//         vector<int> point_search_Idx;
//         vector<float> point_search_dis;
//         line_pa_pb_set_.clear();
//         for(int pt_idx =0; pt_idx<p_current_frame_->pc_cornerPt_sample_->points.size();pt_idx++){
//             tranformPoint(p_current_frame_->pc_cornerPt_sample_->points[pt_idx], pt_tranform, T1);
//             // 从上一帧中寻找最近邻点
//             p_kdtree_corner_->nearestKSearch(pt_tranform, 3, point_search_Idx, point_search_dis);
//             // 确保搜索点足够近
//             if(point_search_dis[2] < max_dis_plane_for_match_)
//             {
//                 std::vector<Eigen::Vector3f> nearCorners;
//                 Eigen::Vector3f center(0.0, 0.0, 0.0);
//                 for(int j=0; j<3; j++)
//                 {
//                     Eigen::Vector3f tmp(
//                         p_last_frame_->pc_corner_->points[point_search_Idx[j]].x,
//                         p_last_frame_->pc_corner_->points[point_search_Idx[j]].y,
//                         p_last_frame_->pc_corner_->points[point_search_Idx[j]].z
//                     );
//                     center = center + tmp;
//                     nearCorners.push_back(tmp);
//                 }
//                 center = center / 3.0;

//                 Eigen::Matrix3f covMat = Eigen::Matrix3f::Zero();
//                 for(int j=0; j<3; j++)
//                 {
//                     Eigen::Vector3f tmpZeroMean = 
//                         nearCorners[j] - center;
//                     covMat = covMat + tmpZeroMean * tmpZeroMean.transpose();
//                 }

//                 // SVD分解协方差，如果最大特征值远大于其余两个特征值，说明分布成直线
//                 // 最大特征值对应特征向量即为直线的方向
//                 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> saes(covMat);
//                 // note Eigen library sort eigenvalues in increasing order
//                 Eigen::Vector3f unit_direction = saes.eigenvectors().col(2);
//                 if(saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1])   //这个必须判断 不然容易出现奇异值
//                 {
//                     // 根据直线方向和中心点，在直线上找两个点
//                     Eigen::Vector3d point_on_line(center[0],center[1],center[2]);
//                     // 提取到的直线上的两个点，用于确定方向
//                     Eigen::Vector3d last_point_a, last_point_b;
//                     last_point_a = 0.1 * Eigen::Vector3d( unit_direction[0], unit_direction[1], unit_direction[2]) + point_on_line;
//                     last_point_b = -0.1 * Eigen::Vector3d( unit_direction[0], unit_direction[1], unit_direction[2]) + point_on_line; 
//                     line_pa_pb_set_.insert(make_pair(pt_idx,make_pair(last_point_a,last_point_b)));
//                 }
//             }
//         }
//     }

//     // cout<<"it "<<iterCount<<endl;
//     PointType coeff;
//     for(int pt_idx =0; pt_idx<p_current_frame_->pc_cornerPt_sample_->points.size();pt_idx++){
//         if(line_pa_pb_set_.find(pt_idx)!=line_pa_pb_set_.end()){
//             tranformPoint(p_current_frame_->pc_cornerPt_sample_->points[pt_idx], pt_tranform, T1);
//             Eigen::Vector3d tmp_a = line_pa_pb_set_[pt_idx].first;
//             Eigen::Vector3d tmp_b = line_pa_pb_set_[pt_idx].second;
//             Eigen::Vector3d tmp_p(pt_tranform.x, pt_tranform.y, pt_tranform.z);
//             Eigen::Matrix<double, 3, 1> nu = (tmp_p - tmp_a).cross(tmp_p - tmp_b); //(叉乘)
//             Eigen::Matrix<double, 3, 1> de = tmp_a - tmp_b;
//             Eigen::Matrix<double, 3, 1> ld = de.cross(nu);
//             ld.normalize();
//             coeff.x =  ld[0]; 
//             coeff.y = ld[1];
//             coeff.z =  ld[2];
//             coeff.intensity = nu.norm()/de.norm();

//             // cout<<"coeff "<<coeff<<endl;

//             //权重计算，距离越大权重越小，距离越小权重越大，得到的权重范围<=1
//             float s = 1;
//             if (iterCount >= 5) { //5次迭代之后开始增加权重因素
//                 //增加权重，距离越远，影响影子越小
//                 s = 1 - 1.8 * fabs(coeff.intensity);
//             }

//             if (s > 0.1 && coeff.intensity != 0) {
//                 coeff.x = s * coeff.x;
//                 coeff.y = s * coeff.y;
//                 coeff.z = s * coeff.z;
//                 coeff.intensity = s * coeff.intensity;
//                 // 特征点指到特征直线的垂线方向，其中intensity存储了点到直线的距离，也就是残差
//                 coeffSel_->push_back(coeff);
//                 laserCloudOri_->push_back(p_current_frame_->pc_cornerPt_sample_->points[pt_idx]);
//             }
//         }
//         // ROS_DEBUG_STREAM("tmp a "<<tmp_a<<"  tmp b "<<tmp_b<<"tmp p "<<tmp_b);
//         // ROS_DEBUG_STREAM("nu  "<<nu<<" de  "<<de<<"ld  "<<ld);
//     }
// }

// bool Frontend::calCornerJacobi(int iterCount){
//     int pointSelNum = laserCloudOri_->points.size();
//     // cout<<"point num "<<pointSelNum<<endl;
//     Eigen::Matrix<double, 1, 3> coeff;
//     Eigen::Matrix<double,3,3> m_temp,H;
//     Eigen::Vector3d pointOri,J_t_B,delta_x;
//     Eigen::Matrix<double, 3, 3> matP = Eigen::Matrix<double, 3, 3>::Zero();
//     Eigen::MatrixXd J,B;
//     J=Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>();
//     J.resize(pointSelNum,3);
//     B=Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>();
//     B.resize(pointSelNum,1);

//     // double s1 =  sin(test_rpy_[0]);
//     // double c1 =  cos(test_rpy_[0]);
//     // double s2 =  sin(test_rpy_[1]);
//     // double c2 =  cos(test_rpy_[1]);
//     // double s3 =  sin(test_rpy_[2]);
//     // double c3 =  cos(test_rpy_[2]);

//     // m_temp(0,0) = -c2*s3; m_temp(0,1) = -c2*c3;  m_temp(0,2) = 0;
//     // m_temp(1,0) = -s1*s2*s3+c1*c3; m_temp(1,1) = -s1*s2*c3-c1*s3;  m_temp(1,2)=0;
//     // m_temp(2,0) = c1*s2*s3+s1*c3; m_temp(2,1) = c1*s2*c3-s1*s3; m_temp(2,2) =0;

//     float s1 = sin(test_rpy_[0]);
//     float c1 = cos(test_rpy_[0]);
//     float s2 = sin(test_rpy_[1]);
//     float c2 = cos(test_rpy_[1]);
//     float s3 = sin(test_rpy_[2]);
//     float c3 = cos(test_rpy_[2]);
//     float tx = t_last_cur_[0];
//     float ty = t_last_cur_[1];
//     float tz = t_last_cur_[2];

//     Eigen::Matrix3d dm,m;

//     dm(0,0) = -c2*s3; dm(0,1) = -c2*c3;  dm(0,2) = 0;
//     dm(1,0) = -s1*s2*s3+c1*c3; dm(1,1) = -s1*s2*c3-c1*s3;  dm(1,2)=0;
//     dm(2,0) = c1*s2*s3+s1*c3; dm(2,1) = c1*s2*c3-s1*s3; dm(2,2) =0;

//     m(0,0) = c2*c3; m(0,1) = -c2*s3;  m(0,2) = s2;
//     m(1,0) = s1*s2*c3+c1*s3; m(1,1) = -s1*s2*s3+c1*c3;  m(1,2)=-s1*c2;
//     m(2,0) = -c1*s2*c3+s1*s3; m(2,1) = c1*s2*s3+s1*c3; m(2,2) =c1*c2;

//     // 构建雅可比矩阵，求解
//     for (int i = 0; i < pointSelNum; i++) {
//         // pointOri[0] = laserCloudOri_->points[i].x;
//         // pointOri[1] = laserCloudOri_->points[i].y;
//         // pointOri[2] = laserCloudOri_->points[i].z;
//         // coeff[0] = coeffSel_->points[i].x;
//         // coeff[1] = coeffSel_->points[i].y;
//         // coeff[2] = coeffSel_->points[i].z;
//         // cout<<"coeff raw "<<coeffSel_->points[i]<<endl;
//         // cout<<"pointOri "<<pointOri<<endl;

//         // anti_symmetric(q_last_cur_.toRotationMatrix()*pointOri, m_temp);
//         // m_temp = -m_temp;
//         // coeff = coeff*m_temp;

//         // anti_symmetric(pointOri, m_temp);
//         // cout<<"m_temp ant "<<m_temp<<endl;
//         // m_temp = -q_last_cur_.toRotationMatrix()*m_temp;
//         // cout<<"q_last cur   "<<q_last_cur_.toRotationMatrix()<<endl;
//         // coeff = coeff*m_temp;

//         // coeff[2] = coeff*m_temp*pointOri;

//         // cout<<"coeff "<<coeff<<"  "<<coeff[2]<<endl;


//         Eigen::Vector3d pt(laserCloudOri_->points[i].x,laserCloudOri_->points[i].y,laserCloudOri_->points[i].z);
//         Eigen::Vector3d t(tx,ty,tz);
//         Eigen::Vector3d dDdG(coeffSel_->points[i].x,coeffSel_->points[i].y,coeffSel_->points[i].z);
        
//         J(i,0) = dDdG.transpose()*dm*(pt-t);
//         J(i,1) = dDdG.transpose()*(-m*Eigen::Vector3d(1,0,0));
//         J(i,2) = dDdG.transpose()*(-m*Eigen::Vector3d(0,1,0));
//         B(i, 0) = -0.05*coeffSel_->points[i].intensity;   //原代码乘了-0.05

//         // J(i,0) = coeff[2];
//         // J(i,1) = coeffSel_->points[i].x;
//         // J(i,2) = coeffSel_->points[i].y;
//         // B(i, 0) = -0.05* coeffSel_->points[i].intensity;   //原代码乘了-0.05
//         // cout<<"loss "<<coeffSel_->points[i].intensity<<endl;
//     }
//     H = J.transpose()*J;
//     J_t_B = J.transpose()*B;
//     delta_x = H.colPivHouseholderQr().solve(J_t_B);

//     // cout<<"it "<<iterCount<<endl;
//     // cout<<"corner delta x "<<delta_x<<endl;

//     if(iterCount == 0)
//     {
//         Eigen::Matrix<double, 1, 3> matE = Eigen::Matrix<double, 1, 3>::Zero();
//         Eigen::Matrix<double, 3, 3> matV = Eigen::Matrix<double, 3, 3>::Zero();
//         Eigen::Matrix<double, 3, 3> matV2 = Eigen::Matrix<double, 3, 3>::Zero();

//         // 计算At*A的特征值和特征向量
//         // 特征值存放在matE，特征向量matV
//         Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigenSolver(H);
//         matE = eigenSolver.eigenvalues();
//         matV = eigenSolver.eigenvectors();

//         matV2 = matV;
//         // 退化的具体表现是指什么？
//         is_degenerate_ = false;
//         double eignThre[3] = {10, 10, 10};
//         for (int i = 2; i >= 0; i--) {
//             if (matE(0, i) < eignThre[i]) {
//                 for (int j = 0; j < 3; j++) {
//                     matV2(i, j) = 0;
//                 }
//                 // 存在比10小的特征值则出现退化
//                 is_degenerate_ = true;
//             } else {
//                 break;
//             }
//         }
//         matP = matV.inverse() * matV2;
//     }

//     if (is_degenerate_) {
//         Eigen::Matrix<double, 3, 1> matX2 = Eigen::Matrix<double, 3, 1>::Zero();
//         // matX.copyTo(matX2);
//         matX2 = delta_x;
//         delta_x = matP * matX2;
//     }

//     for(int i=0; i<3; i++){
//         if(isnan(delta_x[i])||abs(delta_x[i])>0.1)
//             delta_x[i]=0;
//     }

//     // cout<<"judge corner delta x "<<delta_x<<endl;
//     test_rpy_[2] = test_rpy_[2] + delta_x[0];

//     Eigen::AngleAxisd axis_zAngle(delta_x[0], Eigen::Vector3d::UnitZ());
//     q_last_cur_ = (q_last_cur_ * axis_zAngle).normalized();
//     t_last_cur_[0] += delta_x[1];
//     t_last_cur_[1] += delta_x[2];

//     // cout<<"t "<<t_last_cur_<<endl;
//     // cout<<"rpy "<<test_rpy_<<endl;

//     float deltaR = sqrt(
//                         pow(delta_x[0]*180/M_PI, 2));
//     float deltaT = sqrt(
//                         pow(delta_x[1]* 100, 2) +
//                         pow(delta_x[2] * 100, 2));

//     if (deltaR < 0.1 && deltaT < 0.1) {
//         return false;
//     }
//     return true;
// }



}
